/* Integration points (quadratures)
   Jukka Toivanen, Jan Stebel
*/

#include <math.h>
#include "ad.h"
#include "intpoint.h"
#include "quadrature.h"

template<unsigned int dim>
void IntPoint<dim>::init(Mesh *mesh, const Quadrature<dim> *q, int ndof)
{
  imesh = mesh;
  Q     = q;
  indof = ndof;
  
  J_a. resize(dim, 3); 
  IJ_a.resize(3, dim);
  x_a. resize(ndof, 3);
  
  J. resize(dim, 3); 
  IJ.resize(3, dim);
  x. resize(ndof, 3);
  
  SF. resize(ndof, 1);
  GSF.resize(dim, ndof);
  HSF.clear();
  
  dintvec = new int[indof];
  dvec.resize(indof);
  for (int i=0; i<indof; i++) dintvec[i]=i; 
}

template<unsigned int dim>
void IntPoint<dim>::eval(CVec &dofvec, int elem, int offset, CVar &val, CVec &grad, eval_t E)
{
  int dof[indof];
  for (int i=0; i<indof; i++)
  {
    dof[i] = (mapDof(elem, i)==-1)?-1:mapDof(elem, i)+offset;
  }
  evaluate(dofvec, dof, val, grad, E);
}

template<unsigned int dim>
void IntPoint<dim>::evalDG(CVec &dofvec, int elem, int iParent, int offset, CVar &val, CVec &grad, eval_t E)
{
  int dof[indof];
  for (int i=0; i<indof; i++)
  {
    dof[i] = (mapDof(elem, i, iParent)==-1)?-1:mapDof(elem, i, iParent)+offset;
  }
  evaluate(dofvec, dof, val, grad, E);
}

template<unsigned int dim>
void IntPoint<dim>::evalBasis(int basis, CVar &val, CVec &grad, eval_t E)
{
  dvec = 0.0;
  dvec(basis)= 1.0;
  evaluate(dvec, dintvec, val, grad, E);
}

template<unsigned int dim>
void IntPoint<dim>::evalBasisDG(int elem, int parent, int basis, CVar &val, CVec &grad, eval_t E)
{
  dvec = 0.0;
  dvec(basis)= 1.0;
  evaluate(dvec, dintvec, val, grad, E);
}

template<unsigned int dim>
CVar IntPoint<dim>::getX()
{
  CVar _x = 0;
  for (int i=0; i<x.getNrows(); i++) _x += x(i,0)*SF(i,0);
  return _x;
}

template<unsigned int dim>
CVar IntPoint<dim>::getY()
{
  CVar _y = 0;
  for (int i=0; i<x.getNrows(); i++) _y += x(i,1)*SF(i,0);
  return _y;
}

template<unsigned int dim>
CVar IntPoint<dim>::getZ()
{
  CVar _z = 0;
  for (int i=0; i<x.getNrows(); i++) _z += x(i,2)*SF(i,0);
  return _z;
}

template<unsigned int dim>
void IntPoint<dim>::calcJ(eval_t E)
{
  switch (E)
  {
    case EVAL_F:
    case EVAL_FG:
      for (unsigned int i=0; i<dim; i++)
      {
        for (int j=0; j<3; j++)
        {
          J(i,j) = 0;
          for (int k=0; k<indof; k++) J(i,j) += x(k,j)*GSF(i,k);
        }
      }
      break;
    case EVAL_FA:
      for (unsigned int i=0; i<dim; i++)
      {
        for (int j=0; j<3; j++)
        {
          J_a(i,j) = 0;
          for (int k=0; k<indof; k++) J_a(i,j) += x_a(k,j)*GSF(i,k);
        }
      }
      break;
  }
}

template<unsigned int dim>
void IntPoint<dim>::evaluate(CVec &dofvec, int *dof, CVar &val, CVec &grad, eval_t E)
{
  int i, j, k;
  val = 0;
  for (i=0; i<indof; i++) val += dofvec(dof[i])*SF(i,0);
  switch (E)
  {
    case EVAL_F: break;
    case EVAL_FG:
      grad.resize(3);
      for (i=0; i<3; i++)
      {
        grad(i) = 0;
        for (j=0; j<indof; j++)
          for (k=0; k<(int)dim; k++) grad(i) += dofvec(dof[j])*(IJ(i,k)*GSF(k,j));
      }
      break;
    case EVAL_FA:
      grad.resize(3);
      for (i=0; i<3; i++)
      {
        grad(i) = 0;
        for (j=0; j<indof; j++)
          for (k=0; k<(int)dim; k++) grad(i) += dofvec(dof[j])*(IJ_a(i,k)*GSF(k,j));
      }
      break;
  }
}


















template<>
void IntPoint<3>::setX(int elem, eval_t E)
{
  switch (E)
  {
    case EVAL_F:
    case EVAL_FG:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x(i,j)=imesh->getCoord(imesh->getVolumeNode(elem,i),j).getVal();
      break;
    case EVAL_FA:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x_a(i,j)=imesh->getCoord(imesh->getVolumeNode(elem,i),j);
      break;
  }
}

template<>
void IntPoint<3>::calcE(int elem, eval_t E)
{
  setX(elem, E);
  if (isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_inverse_3d<double>(J, IJ, dJ, IdJ);
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_inverse_3d<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        break;
    }
  }
}

template<>
void IntPoint<3>::calcQP(int qp, eval_t E)
{
  ksi[0]  = Q->p[0][qp];
  ksi[1]  = Q->p[1][qp];
  ksi[2]  = Q->p[2][qp];
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_inverse_3d<double>(J, IJ, dJ, IdJ);
        dx = dJ*Q->w[qp];
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_inverse_3d<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*Q->w[qp];
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*Q->w[qp];
  }
  else
  {
    dx = dJ*Q->w[qp];
  }
}

template<>
void IntPoint<3>::calcQP(double l1, double l2, double l3, double w, eval_t E)
{
  ksi[0]  = l1;
  ksi[1]  = l2;
  ksi[2]  = l3;
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_inverse_3d<double>(J, IJ, dJ, IdJ);
        dx = dJ*w;
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_inverse_3d<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*w;
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*w;
  }
  else
  {
    dx = dJ*w;
  }
}

template<>
int IntPoint<3>::mapDof (int elem, int dof)
{
	return imesh->getVolumeNode(elem, dof);
}

template<>
int IntPoint<3>::mapDof(int e, int n, int p)
{
	throw 1;
}







template<>
void IntPoint<2>::setX(int elem, eval_t E)
{
  switch (E)
  {
    case EVAL_F:
    case EVAL_FG:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x(i,j)=imesh->getCoord(imesh->getFaceNode(elem,i),j).getVal();
      break;
    case EVAL_FA:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x_a(i,j)=imesh->getCoord(imesh->getFaceNode(elem,i),j);
      break;
  }
}


template<>
void IntPoint<2>::calcE(int elem, eval_t E)
{
  setX(elem, E);
  if (isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<double>(J, IJ, dJ, IdJ);
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        break;
    }
  }
}

template<>
void IntPoint<2>::calcQP(int qp, eval_t E)
{
  ksi[0]  = Q->p[0][qp];
  ksi[1]  = Q->p[1][qp];
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<double>(J, IJ, dJ, IdJ);
        dx = dJ*Q->w[qp];
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*Q->w[qp];
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*Q->w[qp];
  }
  else
  {
    dx = dJ*Q->w[qp];
  }
}


template<>
void IntPoint<2>::calcQP(double l1, double l2, double l3, double w, eval_t E)
{
  ksi[0] = l1;
  ksi[1] = l2;
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<double>(J, IJ, dJ, IdJ);
        dx = dJ*w;
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_2x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*w;
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*w;
  }
  else
  {
    dx = dJ*w;
  }
}


template<>
int IntPoint<2>::mapDof (int elem, int dof)
{
	return imesh->getFaceNode(elem, dof);
}

template<>
inline int IntPoint<2>::mapDof(int elem, int dof, int iParent)
{
	return imesh->getVolumeNode(imesh->getFaceParentNo(elem, iParent), imesh->getFaceParentNodeNo(elem, iParent, dof));
}


template<>
void IntPoint<1>::setX(int elem, eval_t E)
{
  switch (E)
  {
    case EVAL_F:
    case EVAL_FG:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x(i,j)=imesh->getCoord(imesh->getEdgeNode(elem,i),j).getVal();
      break;
    case EVAL_FA:
      for (int i=0; i<indof; i++)
        for (int j=0; j<3; j++)
          x_a(i,j)=imesh->getCoord(imesh->getEdgeNode(elem,i),j);
      break;
  }
}


template<>
void IntPoint<1>::calcE(int elem, eval_t E)
{
  setX(elem, E);
  if (isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<double>(J, IJ, dJ, IdJ);
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        break;
    }
  }
}


template<>
void IntPoint<1>::calcQP(int qp, eval_t E)
{
  ksi[0]  = Q->p[0][qp];
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<double>(J, IJ, dJ, IdJ);
        dx = dJ*Q->w[qp];
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*Q->w[qp];
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*Q->w[qp];
  }
  else
  {
    dx = dJ*Q->w[qp];
  }
}


template<>
void IntPoint<1>::calcQP(double l1, double l2, double l3, double w, eval_t E)
{
  ksi[0]  = l1;
  
  setSF();
  if (!isLinear())
  {
    switch (E)
    {
      case EVAL_F:
      case EVAL_FG:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<double>(J, IJ, dJ, IdJ);
        dx = dJ*w;
        break;
      case EVAL_FA:
        setGSF();
        calcJ(E);
        mat_pseudoinverse_1x3<CVar>(J_a, IJ_a, dJ_a, IdJ_a);
        dx_a = dJ_a*w;
        break;
    }
  }
  else if (E == EVAL_FA)
  {
    dx_a = dJ_a*w;
  }
  else
  {
    dx = dJ*w;
  }
}


template<>
int IntPoint<1>::mapDof (int elem, int dof)
{
	return imesh->getEdgeNode(elem, dof);
}

template<>
inline int IntPoint<1>::mapDof(int elem, int dof, int iParent)
{
	return imesh->getFaceNode(imesh->getEdgeParentNo(elem, iParent), imesh->getEdgeParentNodeNo(elem, iParent, dof));
}








template class IntPoint<1>;
template class IntPoint<2>;
template class IntPoint<3>;



